#ifndef __GLF_MATH_MTX_H_INCLUDED__
#define __GLF_MATH_MTX_H_INCLUDED__

#include <glf/config.h>
//#include <glf/core/mem.h>

namespace glf {

template<typename T>
struct mtx22 {
	union {
		struct {
			T _00, _01;
			T _10, _11;
		};
		T m[2][2];
		T a[4];
	};
};

template<typename T>
struct mtx33 {
	union {
		struct {
			T _00, _01, _02;
			T _10, _11, _12;
			T _20, _21, _22;
		};
		T m[3][3];
		T a[9];
	};
};


template<typename T>
struct mtx34 {
	union {
		struct {
			T _00, _01, _02, _03;
			T _10, _11, _12, _13;
			T _20, _21, _22, _23;
		};
		T m[3][4];
		T a[12];
	};
};

template<typename T>
struct mtx44 {
	union {
		struct {
			T _00, _01, _02, _03;
			T _10, _11, _12, _13;
			T _20, _21, _22, _23;
			T _30, _31, _32, _33;
		};
		T m[4][4];
		T a[16];
	};
};

template<typename T>
struct Mtx34 : public mtx34<T> {
	typedef Mtx34<T> Mtx34T;
	Mtx34() {
	}
	Mtx34(const T* p);
	Mtx34(const mtx34<T>& rhs);
	//Mtx34(const MTX33& rhs);
	Mtx34(T x00, T x01, T x02, T x03,
		  T x10, T x11, T x12, T x13,
		  T x20, T x21, T x22, T x23)
	{
		this->_00 = x00; this->_01 = x01; this->_02 = x02; this->_03 = x03;
		this->_10 = x10; this->_11 = x11; this->_12 = x12; this->_13 = x13;
		this->_20 = x20; this->_21 = x21; this->_22 = x22; this->_23 = x23;
	}

	// cast operators
	operator T*() { 
		return &this->_00; 
	}
	operator const T*() const { 
		return &this->_00; 
	}

	//operator MtxPtr() { return (MtxPtr)&this->_00; }
   // operator ConstMtxPtr() const { return (ConstMtxPtr)&this->_00; }


	Mtx34T& operator+=(const Mtx34T& rhs){ 
		*this = operator+(rhs); 
		return *this; 
	}
	Mtx34T& operator-=(const Mtx34T& rhs) { 
		*this = operator-(rhs); 
		return *this; 
	}
	
	Mtx34T& operator*=(T f) { 
		*this = *this * f; 
		return *this; 
	}
	Mtx34T& operator/=(T f) { 
		return operator*=(T(1) / f); 
	}

	Mtx34T operator+() const { 
		return *this; 
	}
	Mtx34T operator-() const {
		return Mtx34(-this->_00, -this->_01, -this->_02, -this->_03,
					 -this->_10, -this->_11, -this->_12, -this->_13,
					 -this->_20, -this->_21, -this->_22, -this->_23);
	}

	static Mtx34T Identity() {
		return Mtx34T(T(1), 0, 0, 0,
					  0, T(1), 0, 0,
					  0, 0, T(1), 0);
	}

	T Determinate() const {
		//123,132,213,231,312,321 =>
		//012,021,102,120,201,210
		//+ - - + + -

		return  (this->_00 * this->_11 * this->_22) - (this->_00 * this->_12 * this->_21) - (this->_01 * this->_10 * this->_22) 
			  + (this->_01 * this->_12 * this->_20) + (this->_02 * this->_10 * this->_21) - (this->_02 * this->_11 * this->_20);
	}

	Mtx34T Inverse() const;

	//
	Mtx34T operator+(const Mtx34T& rhs) const;
	Mtx34T operator-(const Mtx34T& rhs) const;
	Mtx34T operator*(T f) const;

	Mtx34T operator/(T f) const { return *this * (T(1) / f); }

	bool operator==(const Mtx34T& rhs) const { 
		return memcmp(this, &rhs, sizeof(Mtx34T)) == 0; 
	}
	bool operator!=(const Mtx34T& rhs) const  { 
		return memcmp(this, &rhs, sizeof(Mtx34T)) != 0; 
	}

	vec3<T> operator*(const vec3<T>& v) const;

	//rotate around X axis in radians
	static Mtx34T RotateX(T rad);
	static Mtx34T RotateY(T rad);
	static Mtx34T RotateZ(T rad);
	static Mtx34T RotateAxis(const vec3<T>& axis, T rad);
	
	Mtx34T Transpose() const;

};

template<typename T>
Mtx34<T> Mtx34<T>::Inverse() const
{ 
	Mtx34T retval;

	// 1/det(a) * [ ...]
	T det = Determinate();
	
	ASSERT(det);

	// compute the inverse of the upper submatrix:

	// find the transposed matrix of cofactors of the upper submatrix
	// and multiply by (1/det)

	det = (T)1 / det;

	retval.m[0][0] =  (this->m[1][1]*this->m[2][2] - this->m[2][1]*this->m[1][2]) * det;
	retval.m[0][1] = -(this->m[0][1]*this->m[2][2] - this->m[2][1]*this->m[0][2]) * det;
	retval.m[0][2] =  (this->m[0][1]*this->m[1][2] - this->m[1][1]*this->m[0][2]) * det;

	retval.m[1][0] = -(this->m[1][0]*this->m[2][2] - this->m[2][0]*this->m[1][2]) * det;
	retval.m[1][1] =  (this->m[0][0]*this->m[2][2] - this->m[2][0]*this->m[0][2]) * det;
	retval.m[1][2] = -(this->m[0][0]*this->m[1][2] - this->m[1][0]*this->m[0][2]) * det;

	retval.m[2][0] =  (this->m[1][0]*this->m[2][1] - this->m[2][0]*this->m[1][1]) * det;
	retval.m[2][1] = -(this->m[0][0]*this->m[2][1] - this->m[2][0]*this->m[0][1]) * det;
	retval.m[2][2] =  (this->m[0][0]*this->m[1][1] - this->m[1][0]*this->m[0][1]) * det;


	// compute (invA)*(-C)
	retval.m[0][3] = -retval.m[0][0]*this->m[0][3] - retval.m[0][1]*this->m[1][3] - retval.m[0][2]*this->m[2][3];
	retval.m[1][3] = -retval.m[1][0]*this->m[0][3] - retval.m[1][1]*this->m[1][3] - retval.m[1][2]*this->m[2][3];
	retval.m[2][3] = -retval.m[2][0]*this->m[0][3] - retval.m[2][1]*this->m[1][3] - retval.m[2][2]*this->m[2][3];

	return retval;
}

template<typename T>
Mtx34<T> Mtx34<T>::operator+(const Mtx34<T>& rhs) const
{
	Mtx34T retval;

	for(int i=0; i< 12; i++)
	{
		retval.a[i] = this->a[i] + rhs.a[i];
	}

	return retval;
}

template<typename T>
Mtx34<T> Mtx34<T>::operator-(const Mtx34<T>& rhs) const
{
	Mtx34T retval;

	for(int i=0; i< 12; i++)
	{
		retval.a[i] = this->a[i] - rhs.a[i];
	}

	return retval;
}

template<typename T>
Mtx34<T> Mtx34<T>::operator*(T f) const
{
	//scalar multiple.. multiple every value by f
	Mtx34T retval;
	for(int i=0; i< 12; i++)
	{
		retval.a[i] = this->a[i] *f;
	}
	return retval;
}

template<typename T>
vec3<T> Mtx34<T>::operator*(const vec3<T>& v) const 
{
	//mtx-vector cross product
	vec3<T> dst;
	 // A Vec3 has a 4th implicit 'w' coordinate of 1
	dst.x = this->m[0][0]*v.x + this->m[0][1]*v.y + this->m[0][2]*v.z + this->m[0][3];
	dst.y = this->m[1][0]*v.x + this->m[1][1]*v.y + this->m[1][2]*v.z + this->m[1][3];
	dst.z = this->m[2][0]*v.x + this->m[2][1]*v.y + this->m[2][2]*v.z + this->m[2][3];
	//and we ignore the 4th column
	return dst;
}

//rotate around X axis in radians
template<typename T>
Mtx34<T> Mtx34<T>::RotateX(T rad)
{
	T sinA = Sin(rad);
	T cosA = Cos(rad);

	Mtx34T rotatemtx;
	rotatemtx.m[0][0] = (T)1;
	rotatemtx.m[1][1] =  cosA;  rotatemtx.m[1][2] = -sinA;
	rotatemtx.m[2][1] =  sinA;  rotatemtx.m[2][2] =  cosA; 

	return rotatemtx;

}

template<typename T>
Mtx34<T> Mtx34<T>::RotateY(T rad)
{
	T sinA = Sin(rad);
	T cosA = Cos(rad);
	Mtx34T rotatemtx;
	rotatemtx.m[0][0] =  cosA;  rotatemtx.m[0][2] = sinA;
	rotatemtx.m[1][1] = (T)1;
	rotatemtx.m[2][0] =  -sinA;  rotatemtx.m[2][2] =  cosA; 

	return rotatemtx;
}

template<typename T>
Mtx34<T> Mtx34<T>::RotateZ(T rad)
{
	T sinA = Sin(rad);
	T cosA = Cos(rad);
	Mtx34T rotatemtx;
	rotatemtx.m[0][0] =  cosA;  rotatemtx.m[0][1] = -sinA;
	rotatemtx.m[1][0] =  sinA;  rotatemtx.m[1][1] =  cosA; 
	rotatemtx.m[2][2] = (T)1;
	return rotatemtx;
}

template<typename T>
Mtx34<T> Mtx34<T>::RotateAxis(const vec3<T>& axis, T rad)
{ 
	//rotation about an arbitrary axis
	T s = Sin(rad); // sinTheta
	T c = Cos(rad); // cosTheta
	T t = 1.0f - c; // ( 1 - cosTheta )

    T xSq, ySq, zSq;    // x, y, z squared

	Vec3<T> a = axis;

	a.Normalize();
    xSq = a.x * a.x;
    ySq = a.y * a.y;
    zSq = a.z * a.z;

	Mtx34T rotatemtx;

	rotatemtx.m[0][0] = ( t * xSq )   + ( c );
	rotatemtx.m[0][1] = ( t * a.x * a.y ) - ( s * a.z );
	rotatemtx.m[0][2] = ( t * a.x * a.z ) + ( s * a.y );
	rotatemtx.m[0][3] = T(0);

	rotatemtx.m[1][0] = ( t * a.x * a.y ) + ( s * a.z );
	rotatemtx.m[1][1] = ( t * ySq )   + ( c );
	rotatemtx.m[1][2] = ( t * a.y * a.z ) - ( s * a.x );
	rotatemtx.m[1][3] = T(0);

	rotatemtx.m[2][0] = ( t * a.x * a.z ) - ( s * a.y );
	rotatemtx.m[2][1] = ( t * a.y * a.z ) + ( s * a.x );
	rotatemtx.m[2][2] = ( t * zSq )   + ( c );
	rotatemtx.m[2][3] = T(0);

	return rotatemtx;

}

template<typename T>
Mtx34<T> Mtx34<T>::Transpose() const
{

	//computes the transpose of a matrix.
    //As matrices are 3x4, fourth column (translation component) is
    //lost and becomes (0,0,0).
	//This function is intended for use in computing an
    //inverse-transpose matrix to transform normals for lighting.
    //In this case, lost translation component doesn't matter.

	return Mtx34T(
			this->m[0][0],   this->m[1][0],      this->m[2][0]     ,T(0),
			this->m[0][1],   this->m[1][1],      this->m[2][1]     ,T(0),
			this->m[0][2],   this->m[1][2],      this->m[2][2]     ,T(0)
			);

}

template<typename T>
struct Mtx44 : public mtx44<T>
{
	
};

typedef mtx34<float> mtx34f;
typedef Mtx34<float> Mtx34f;

typedef mtx44<float> mtx44f;
typedef Mtx44<float> Mtx44f;

}

#endif
